using System;
using System.Collections.Generic;
using System.Text;
using Diversity.Maths;

namespace Diversity.Robotics.Navigation
{
    /// <summary>
    /// CURRENTLY UNUSED ... (NOT TESTED)
    /// </summary>
    public class OdometryMotionModel : IMotionModel
    {
        #region Private Members
        private double _tErr1;
        private double _tErr2;
        private double _rErr1;
        private double _rErr2;
        private double _dRot1;
        private double _dRot2;
        private double _dTrans;
      //  private ProbabilityUtils ProbabilityUtils;
        #endregion

        #region Public Members
        public double tErr1
        {
            get { return _tErr1; }
            set { _tErr1 = value; }
        }

        public double tErr2
        {
            get { return _tErr2; }
            set { _tErr2 = value; }
        }

        public double rErr1
        {
            get { return _rErr1; }
            set { _rErr1 = value; }
        }

        public double rErr2
        {
            get { return _rErr2; }
            set { _rErr2 = value; }
        }


        #endregion

        public OdometryMotionModel(double terr1, double terr2, double rerr1, double rerr2)
        {
          //  ProbabilityUtils = new ProbabilityUtils();
            tErr1 = terr1;
            tErr2 = terr2;
            rErr1 = rerr1;
            rErr2 = rerr2;
        }

        public void PrepareForSampling(Pose2D odoState, Pose2D odoStatePrev)
        {
            _dRot1 = Math.Atan2(odoState.Y - odoStatePrev.Y, odoState.X - odoStatePrev.X) - odoStatePrev.Theta;

            _dRot1 = LimitTheta(_dRot1);
           
            _dTrans =
                Math.Sqrt(Math.Pow(odoState.Y - odoStatePrev.Y, 2) + Math.Pow(odoState.X - odoStatePrev.X, 2));
            _dRot2 = odoState.Theta - odoStatePrev.Theta - _dRot1;

            _dRot2 = LimitTheta(_dRot2);
            
        }

        public Pose2D GetSample(Pose2D state)
        {
            double dHatRot1 = _dRot1 - ProbabilityUtils.RandomFromGaussian(_rErr1 * Math.Abs(_dRot1) + _tErr1 * _dTrans);
            double dHatTrans = _dTrans -
                               ProbabilityUtils.RandomFromGaussian(_tErr2 * _dTrans + _rErr2 * (Math.Abs(_dRot1) + Math.Abs(_dRot2)));
            double dHatRot2 = _dRot2 - ProbabilityUtils.RandomFromGaussian(_rErr1 * Math.Abs(_dRot2) + _tErr1 * _dTrans);

            double x = state.X + dHatTrans * Math.Cos(state.Theta + dHatRot1);
            double y = state.Y + dHatTrans * Math.Sin(state.Theta + dHatRot1);
            double theta =  state.Theta + dHatRot1 + dHatRot2;
            theta = LimitTheta(theta);
           
            
            return new Pose2D(x, y, theta);
        }

        public double GetProbabilityOfSample(Pose2D newState, Pose2D oldState)
        {
            double dHatRot1 = Math.Atan2(newState.Y - oldState.Y, newState.X - oldState.X) - oldState.Theta;
            dHatRot1 = LimitTheta(dHatRot1);
            double dHatTrans = Math.Sqrt(Math.Pow(newState.X - oldState.X, 2) + Math.Pow(newState.Y - oldState.Y, 2));
            double dHatRot2 = newState.Theta - oldState.Theta - dHatRot1;
            dHatRot2 = LimitTheta(dHatRot2);
            
            double p1 =
                ProbabilityUtils.ProbabilityOfValInVariance(_dRot1 - dHatRot1, _rErr1 * Math.Abs(dHatRot1) + _tErr1 * dHatTrans);
            double p2 =
                ProbabilityUtils.ProbabilityOfValInVariance(_dTrans - dHatTrans,
                                                      _tErr2 * dHatTrans +
                                                      _rErr2 * (Math.Abs(Math.Abs(dHatRot1) + Math.Abs((dHatRot2)))));
            double p3 =
                ProbabilityUtils.ProbabilityOfValInVariance(_dRot2 - dHatRot2, _rErr1 * Math.Abs(dHatRot2) + _tErr1 * dHatTrans);
            if (p1 == 0 || p2 == 0 || p3 == 0 || double.IsNaN(p1) || double.IsNaN(p2) || double.IsNaN(p3))
            {
                Console.Out.WriteLine("Problem with weights: " + p1 + " " + p2 + " " + p3);
                Console.Out.WriteLine("dHatRot1: " + dHatRot1 + " dHatRot2" + dHatRot2 + " _dRot1" + _dRot1);
            }
            return p1 * p2 * p3;
        }

        private static double LimitTheta(double theta)
        {
            while (theta > Math.PI) { theta -= Math.PI * 2; };
            while (theta < -Math.PI) { theta += Math.PI * 2; };
            return theta;
        }

        #region IMotionModel Members

        /// <summary>
        /// For the odometry model, this returns the state you give it!
        /// </summary>
        /// <param name="state"></param>
        /// <returns></returns>
        public Pose2D GetPerfectSample(Pose2D state)
        {
            return state;
        }

        /// <summary>
        /// Returns 'Odometry'
        /// </summary>
        /// <returns></returns>
        public string ModelType()
        {
            return "Odometry";
        }

        /// <summary>
        /// NOT IMPLEMENTED FOR THIS MODEL TYPE
        /// </summary>
        /// <param name="velocityOfAction"></param>
        /// <param name="timeOfAction"></param>
        public void PrepareForSampling(Velocity velocityOfAction, TimeSpan timeOfAction)
        {
            throw new Exception("The method or operation is not implemented.");
        }

        #endregion
    }
}
